clc
inlier_noise_level = 5; %deg;
n_samples = 2;
R_samples = cell(1, n_samples);
R_true = RandomRotation(pi); 

for i = 1:n_samples
    % Inliers: perturb by 5 deg.
    axis_perturb = rand(3,1)-0.5;
    axis_perturb = axis_perturb/norm(axis_perturb);
    angle_perturb = normrnd(0,inlier_noise_level/180*pi); 
    if (angle_perturb==0)
        R_perturb = eye(3);
    else
        so3 = [0 -axis_perturb(3) axis_perturb(2) ; axis_perturb(3) 0 -axis_perturb(1) ; -axis_perturb(2) axis_perturb(1) 0 ];
        R_perturb = eye(3)+so3*sin(angle_perturb)+so3^2*(1-cos(angle_perturb));
    end
    R_samples{i} = R_perturb*R_true;
end

b_outlier_rejection = true;     % 是否忽略 outlier
n_iterations = 10;              % 迭代几次
thr_convergence = 0.001;        % 判断迭代时是否收敛

R_chordal = ChordalL1Mean(R_samples, b_outlier_rejection, n_iterations, thr_convergence);

%%
function R = RandomRotation(max_angle_rad)

    unit_axis = rand(3,1)-0.5;
    unit_axis = unit_axis/norm(unit_axis);
    angle = rand*max_angle_rad;
    if (angle==0)
        R = eye(3);
    else
        so3 = [0 -unit_axis(3) unit_axis(2) ; unit_axis(3) 0 -unit_axis(1) ; -unit_axis(2) unit_axis(1) 0 ];
        R = eye(3)+so3*sin(angle)+so3^2*(1-cos(angle));
    end

end
